/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-15 03:26:31
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 05:10:18
 * @Description:
 * @Reference:
 */
#ifndef _VILENS_PIPELINE_ONLINE_H_
#define _VILENS_PIPELINE_ONLINE_H_

#include "lci_slam/vilens/common_header.h"

#include "lci_slam/vilens/camera/camera_processor.h"
#include "lci_slam/vilens/imu/imu_processor.h"
#include "lci_slam/vilens/lidar/lidar_preprocessor.h"
#include "lci_slam/vilens/lidar/lidar_processor.h"
#include "lci_slam/vilens/utility/config.h"
#include "lci_slam/vilens/utility/helper.h"

namespace vilens {

class onlinePipeline {
public:
    typedef std::shared_ptr<onlinePipeline> Ptr;
    explicit onlinePipeline(const std::string &config_path);
    ~onlinePipeline() {}

    int process();
    void reset();

    void loadParameter(const std::string &config_path);
    void allocateMemory();
    void registerPublisher();

    // sensor data callback
    void imuCallback(const sensor_msgs::ImuConstPtr &imu_msg);  // imu
    void standardPclCallback(
        const sensor_msgs::PointCloud2::ConstPtr &msg);  // standard_pcl
    void livoxPclCallback(
        const lci_slam::CustomMsg::ConstPtr &msg);  // livox_pcl

    void monocularCallback(const sensor_msgs::ImageConstPtr &img_msg,
                           int cam_id);
    void stereoCallback(const sensor_msgs::ImageConstPtr &img_msg0,
                        const sensor_msgs::ImageConstPtr &img_msg1, int cam_id0,
                        int cam_id1);

    void restartCallback(const std_msgs::BoolConstPtr &restart_msg);
    // void relocalizationCallback(const sensor_msgs::PointCloudConstPtr
    // &points_msg);

    // config setting
    config::Ptr pipeline_config;

protected:
    int lidar_process();

    ros::NodeHandle nh;
    ros::Subscriber subImu;
    ros::Subscriber subRestart;
    ros::Subscriber subLidar;
    std::condition_variable sigBuffer;

    // for Stereo
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                            sensor_msgs::Image>
        syncPol;
    std::vector<std::unique_ptr<message_filters::Synchronizer<syncPol>>>
        syncCam;
    std::vector<
        std::unique_ptr<message_filters::Subscriber<sensor_msgs::Image>>>
        syncSubCam;
    // for Mono
    std::vector<ros::Subscriber> subImages;

    // imu
    imuProcessor::Ptr imu_process_ptr;
    std::mutex mtx_data_buffer;
    std::queue<std::pair<double, sensor_msgs::ImuConstPtr>> imu_data_buffer;
    double last_timestamp_imu = -1;

    // lidar
    lidarPreprocessor::Ptr lidar_preprocess_ptr;
    lidarProcessor::Ptr lidar_process_ptr;
    std::mutex mtx_data_buffer;
    std::queue<std::pair<double, PointCloudXYZI::Ptr>> lidar_data_buffer;
    double last_timestamp_lidar = -1;
    double delta_time_imu_lidar = 0.0;

    // img
    std::mutex mtx_data_buffer;
    cameraProcessor::Ptr camera_process_ptr;
    double delta_time_img_lidar = 0.0;
};

}  // namespace vilens

#endif